#include <rclcpp/rclcpp.hpp>
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include <sensor_msgs/msg/imu.hpp>
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include <mutex>
#include <cmath> // 包含cos和sin函数的头文件
#include "desired_cordinate.h"
#include "rpy_filter.h"
#include "rpy_filter_initialize.h"
#include <fstream>

//工作文件夹建立文件pos_data_w.txt (r,p,y,测量vL,测量vR,期望vL,期望vR,期望x,期望y,期望wz,观测x,观测y,观测wz)

// float d = 0.07, b = 0.098;//履带参数
float d = 0.085, b = 0.128;
float vLo = 0.0, vRo = 0.0, vLd = 0.0, vRd = 0.0;
double vo = 0.0, wo = 0.0, vd = 0.0, wd = 0.0;

int i = 0, ik = 10, ij=0;
double roll, pitch, yaw;
double roll_in, pitch_in, yaw_in;
double roll_f, pitch_f, yaw_f;
double R=0.1,Q=0.01;

double xo = 0.0, yo = 0.0, zo = 0.0;
double wxo = 0.0, wyo = 0.0, wzo = 0.0;
double xd = 0.0, yd = 0.0, wzd = 0.0;

float R2, R5, R9;
float ekf_pr;

class PosDataV3 : public rclcpp::Node
{
    private:
        std::ofstream data_file_;  // 用于写入数据的文件流对象
        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
        // 声名定时器指针
        rclcpp::TimerBase::SharedPtr timer_;
        // 声明时间
        rclcpp::Time last_time_;
        tf2::Quaternion initial_orientation;
        tf2::Quaternion q_adjusted;
        tf2::Quaternion q_kalman;
        tf2::Vector3 obs_linear_dis;
        tf2::Vector3 obs_angular_dis;
        tf2::Matrix3x3 m_pre;
        std::mutex mutex_;

        void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
        {
            // std::lock_guard<std::mutex> lock(mutex_);
            // 获取IMU的旋转四元数
            tf2::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
            if (i <= ik)
            {
                if(i>4)
                {
                    // 将四元数转换为3x3矩阵
                    tf2::Matrix3x3 m(q);
                    m.getRPY(roll, pitch, yaw);
                    roll_in = roll_in + roll;
                    pitch_in = pitch_in + pitch;
                    yaw_in = yaw_in + yaw;
                    ij=ij+1;
                    if (i == ik)
                    {
                        roll_in = roll_in / (double)ij;
                        pitch_in = pitch_in /(double)ij;
                        yaw_in = yaw_in / (double)ij;
                        initial_orientation.setRPY(-roll_in, -pitch_in, -yaw_in);
                        tf2::Matrix3x3 m_init(initial_orientation);
                        m_pre=m_init;
                        RCLCPP_INFO(this->get_logger(), "得到初始补偿角: Roll: %f, Pitch: %f, Yaw: %f", roll_in, pitch_in, yaw_in);
                    }
                }
                i = i + 1;
            }
            else
            {
                // 计算调整后的四元数
                q_adjusted = initial_orientation * q;
                wo = msg->angular_velocity.z;
            }
        }
        // 声明话题发布者
        rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publish_pos;
        rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publish_rpy;
        void timer_callback()
        {
            std::lock_guard<std::mutex> lock(mutex_);
            // 获取当前时间戳
            rclcpp::Time current_time = rclcpp::Clock().now();
            // 计算当前时间与上一次时间戳的差值
            rclcpp::Duration duration = current_time - last_time_;
            // 将时间间隔转换为秒
            double dt = duration.seconds();
            RCLCPP_INFO(this->get_logger(), "时间间隔dt=%f", dt);
            // 更新上一次的时间戳为当前时间
            last_time_ = current_time;
            if (i > ik)
            {
                // 打印调整后的四元数的RPY角度
                tf2::Matrix3x3 m(q_adjusted);
                m.getRPY(roll, pitch, yaw);
                RCLCPP_INFO(this->get_logger(), "当前RPY: roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw);
                if (std::isnan(roll)||std::isnan(pitch)||std::isnan(yaw))
                {
                    RCLCPP_INFO(this->get_logger(), "na值, 不往下执行");
                }
                else
                {
                    rpy_filter(roll,pitch,yaw,R,Q,&roll_f,&pitch_f,&yaw_f);
                    q_kalman.setRPY(roll_f,pitch_f,yaw_f);
                    tf2::Matrix3x3 m_kalman(q_kalman);
                    std_msgs::msg::Float32MultiArray rpy_data;
                    rpy_data.data.resize(3);
                    rpy_data.data[0] = (float)roll;
                    rpy_data.data[1] = (float)pitch;
                    rpy_data.data[2] = (float)yaw;
                    command_publish_rpy->publish(rpy_data);
                    RCLCPP_INFO(this->get_logger(), "滤波后rpy, roll_f=%f, pitch_f=%f, yaw_f=%f",roll_f,pitch_f,yaw_f);
                    //
                    RCLCPP_INFO(this->get_logger(), "测得速度, vL=%f, vR=%f",vLo,vRo);
                    //写入rpy
                    data_file_ << roll << ",";
                    data_file_ << pitch << ",";
                    data_file_ << yaw << ",";
                    //写入测量速度
                    data_file_ << vLo << ",";
                    data_file_ << vRo << ",";
                    //
                    vo = (vLo + vRo) / 2.0;
                    wo=(vRo-vLo)/(2.0*b);
                    // 创建线速度向量(vx, vy, vz)
                    tf2::Vector3 vo_vec(vo, 0.0, 0.0);
                    // 创建角速度向量(wx, wy, wz)
                    tf2::Vector3 wo_vec(0.0, 0.0, wo);
                    // 将旋转矩阵与线速度向量相乘，得到变换后的线速度向量
                    tf2::Vector3 vo_transformed = m_pre * vo_vec;
                    // 计算相对于世界坐标系的角速度
                    tf2::Vector3 wo_transformed = m_pre * wo_vec;
                    // 世界角速度
                    RCLCPP_INFO(this->get_logger(), "世界坐标系角速度 wx: %f, wy: %f, wz: %f", wo_transformed[0], wo_transformed[1], wo_transformed[2]);
                    // 世界线速度
                    RCLCPP_INFO(this->get_logger(), "世界坐标系线速度 vx: %f, vy: %f, vz: %f", vo_transformed[0], vo_transformed[1], vo_transformed[2]);
                    //
                    /**期望位移计算******************************************************************************/
                    //写入期望速度
                    data_file_ << vLd << ",";
                    data_file_ << vRd << ",";
                    //
                    vd = (vRd + vLd) / 2.0;
                    wd = (vRd - vLd) / (2.0 * b);
                    desired_cordinate(xo, yo, wzo, vd, wd, 0.1, &xd, &yd, &wzd);
                    //写入期望位置
                    data_file_ << xd << ",";
                    data_file_ << yd << ",";
                    data_file_ << wzd << ",";
                    RCLCPP_INFO(this->get_logger(), "期望位置 xd: %f, yd: %f, wzd: %f", vd, wd, wzd);
                    //
                    /**实际位移计算******************************************************************************/
                    //
                    double vx_tr = static_cast<double>(vo_transformed[0]);
                    double vy_tr = static_cast<double>(vo_transformed[1]);
                    double vz_tr = static_cast<double>(vo_transformed[2]);
                    if (abs(vx_tr) >= 0.0001)
                    {
                        xo = xo + vx_tr * dt;
                    }
                    else
                    {
                        xo = xo;
                    }
                    if (abs(vy_tr) >= 0.0001)
                    {
                        yo = yo + vy_tr * dt;
                    }
                    else
                    {
                        yo = yo;
                    }
                    if (abs(vz_tr) >= 0.0001)
                    {
                        zo = zo + vz_tr * dt;
                    }
                    else
                    {
                        zo = zo;
                    }
                    // wxo = static_cast<double>(roll_f);
                    // wyo = static_cast<double>(pitch_f);
                    // wzo = static_cast<double>(yaw_f);
                    double wx_tr = static_cast<double>(wo_transformed[0]);
                    double wy_tr = static_cast<double>(wo_transformed[1]);
                    double wz_tr = static_cast<double>(wo_transformed[2]);
                    if (abs(wx_tr) >= 0.0001)
                    {
                        wxo = wxo + wx_tr * dt;
                    }
                    else
                    {
                        wxo = wxo;
                    }
                    if (abs(wy_tr) >= 0.0001)
                    {
                        wyo = wyo + wy_tr * dt;
                    }
                    else
                    {
                        wyo = wyo;
                    }
                    if (abs(wz_tr) >= 0.0001)
                    {
                        wzo = wzo + wz_tr * dt;
                    }
                    else
                    {
                        wzo = wzo;
                    }
                    RCLCPP_INFO(this->get_logger(), "观测位置 xo: %f, yo: %f, wzo: %f", xo, yo, wzo);
                    //写入期望位置
                    data_file_ << xo << ",";
                    data_file_ << yo << ",";
                    data_file_ << wzo << ",";
                    //
                    //
                    R2 = m_pre[0].x();
                    R5 = m_pre[1].x();
                    R9 = m_pre[2].z();
                    //
                    std_msgs::msg::Float32MultiArray pos_vel_data;
                    pos_vel_data.data.resize(10);
                    pos_vel_data.data[0] = (float)xo;
                    pos_vel_data.data[1] = (float)yo;
                    pos_vel_data.data[2] = (float)wzo;
                    pos_vel_data.data[3] = (float)xd;
                    pos_vel_data.data[4] = (float)yd;
                    pos_vel_data.data[5] = (float)wzd;
                    pos_vel_data.data[6] = R2;
                    pos_vel_data.data[7] = R5;
                    pos_vel_data.data[8] = R9;
                    if(abs(vLo)>0.0001||abs(vRo)>0.0001)
                    {
                        ekf_pr=1.0;
                    }
                    else
                    {
                        ekf_pr=0.0;
                    }
                    pos_vel_data.data[9] =ekf_pr;
                    command_publish_pos->publish(pos_vel_data);
                    m_pre=m;
                    // 都输出完后，写入一个换行符
                    data_file_ << std::endl;
                }
            }
        }
        // 声明一个订阅者L
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_L;
        // 声明一个订阅者R
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_R;
        // 收到话题数据的回调函数L
        void vL_callback(const std_msgs::msg::Float32::SharedPtr msg)
        {
            // std::lock_guard<std::mutex> lock(mutex_);
            float rpmL = msg->data;
            vLo = rpmL * 2 * M_PI / 60.0 * (d / 2.0);
        }
        // 收到话题数据的回调函数R
        void vR_callback(const std_msgs::msg::Float32::SharedPtr msg)
        {
            // std::lock_guard<std::mutex> lock(mutex_);
            float rpmR = msg->data;
            vRo = rpmR * 2 * M_PI / 60.0 * (d / 2.0);
        }
        // 声明一个订阅者vel_g
        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr command_subscribe_vel;
        void velg_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
        {
            // std::lock_guard<std::mutex> lock(mutex_);
            vRd = msg->data[2];
            vLd = msg->data[3];
        }
    public:
        PosDataV3(std::string name) : Node(name)
        {
            RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
            // 打开文件用于写入，文件名为"data_output.txt"
            data_file_.open("pos_data_w.txt", std::ios::out | std::ios::trunc);// 使用trunc模式清空文件内容
            if (!data_file_.is_open())
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to open file for writing.");
            }
            else
            {
                RCLCPP_INFO(this->get_logger(), "File opened successfully for writing data.");
            }
            //订阅imu
            imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "imu/data_raw", 10,
            std::bind(&PosDataV3::imuCallback, this, std::placeholders::_1));
            // 创建定时器，100ms为周期，定时发布
            timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&PosDataV3::timer_callback, this));
            // 创建一个订阅者L订阅话题
            command_subscribe_L = this->create_subscription<std_msgs::msg::Float32>(
                "esp_pro_l", 10, std::bind(&PosDataV3::vL_callback, this, std::placeholders::_1));
            // 创建一个订阅者R订阅话题
            command_subscribe_R = this->create_subscription<std_msgs::msg::Float32>(
                "esp_pro_r", 10, std::bind(&PosDataV3::vR_callback, this, std::placeholders::_1));
            // 初始化上一次时间戳为当前时间
            last_time_ = rclcpp::Clock().now();
            // 创建vel_g订阅者
            command_subscribe_vel = this->create_subscription<std_msgs::msg::Float32MultiArray>(
                "vel_g", 10, std::bind(&PosDataV3::velg_callback, this, std::placeholders::_1));
            // 创建发布速度数组
            command_publish_pos = this->create_publisher<std_msgs::msg::Float32MultiArray>("pos_obs_des", 10);
            //发布rpy
            command_publish_rpy = this->create_publisher<std_msgs::msg::Float32MultiArray>("in_rpy",10);
        }
    ~PosDataV3();// 析构函数声明
};

PosDataV3::~PosDataV3()
{
    if (data_file_.is_open())
    {
        data_file_.close();
        RCLCPP_INFO(this->get_logger(), "File closed successfully.");
    }
}

int main(int argc, char **argv)
{
    static bool initialized = false;
    if (!initialized) {
        rpy_filter_initialize();
        initialized = true;
    }
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<PosDataV3>("pos_data_v3");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}